/* gm_utils.c -- Utilidades diversas
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part os GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#define _GNU_SOURCE 1

#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <ctype.h>
#include <stdarg.h>
#include <sys/types.h>

#include "gm_server.h"

#include "config.h"
#include "gm_utils.h"

/* Static functions */
static void error_message( const int exit_status, const char *mode, const char *format, const va_list arg );

static void
error_message( const int exit_status,
	       const char *mode,
	       const char *format,
	       const va_list arg)
{
    fprintf( stderr, "%s:%s:", PACKAGE_NAME, mode);
    vfprintf( stderr, format, arg);
    fprintf( stderr, "\n");
    fflush( stderr );
    
    if( exit_status >= 0 )
    {
	exit(exit_status);
    }

    return;
}

void
xerror_info( const char *format, ... )
{
    va_list arg;

    va_start (arg, format);
    error_message( -2, "info", format, arg );
    va_end (arg);
    
    return;
}

void
xerror_warning( const char *format, ... )
{
    va_list arg;

    va_start (arg, format);
    error_message( -1, "warning", format, arg );
    va_end (arg);

    return;
}

void
xerror_error( const char *format, ... )
{
    va_list arg;

    va_start (arg, format);
    error_message( -1, "error", format, arg );
    va_end (arg);

    return;
}

void
xerror_fatal( const char *format, ... )
{
    va_list arg;

    va_start (arg, format);
    error_message( EXIT_FAILURE, "FATAL", format, arg );
    va_end (arg);

    return;
}

void *
xmalloc( size_t num )
{
    void *new = malloc( num );

    if( !new )
	xerror_fatal("No memory, AT ALL!!!");

    return new;
}

void *
xrealloc( void *ptr, size_t num )
{
    void *new = NULL;

    if( !ptr )
	return xmalloc( num );

    new = realloc( ptr, num );
    if( !new )
	xerror_fatal("No memory, AT ALL!!!");

    return new;
}

void
xfree( void *ptr )
{
    if( ptr )
    {
	free(ptr);
	ptr = 0;
    }

    return;
}

/** Multiplica duas matrizes.
 *
 * A matriz que conterá o resultado da multiplicação deve conter o
 * tamanho correto e nenhuma verificação quanto a possibilidade da
 * multiplicação é feita.
 *
 * Observar a sequência A x X = B.
 *
 * @param n_row_A Número de linhas da matriz A.
 *
 * @param n_col_A Número de colunas da matriz A.
 *
 * @param **matrix_A Matriz A.
 * 
 * @param n_row_X Número de linhas da matriz X.
 * 
 * @param n_col_X Número de colunas da matriz X.
 * 
 * @param **matrix_X Matriz X.
 *
 * @param **matrix_B Matriz que irá conter o resultado da
 * multiplicação.
 */
void
gm_matrix_mult_raw( const int n_row_A, const int n_col_A, const float ** matrix_A,
		    const int n_row_X, const int n_col_X, const float ** matrix_X,
		    float ** matrix_B )
{
    int i = 0;
    int j = 0;
    int k = 0;

    for( i = 0 ; i < n_row_A ; i++ )
	for( j = 0 ; j < n_col_X ; j++ )
	    for( k = 0 ; k < n_col_A ; k++ )
		matrix_B[i][j] += matrix_A[i][k] * matrix_X[k][j];
}

/** Multiplica duas matrizes.
 *
 * A matriz que conterá o resultado da multiplicação deve conter o
 * tamanho correto e nenhuma verificação quanto a possibilidade da
 * multiplicação é feita.
 *
 * Observar a sequência A x X = B.
 *
 * @param **matrix_A Matriz A.
 * 
 * @param **matrix_X Matriz X.
 *
 * @param **matrix_B Matriz que irá conter o resultado da
 * multiplicação. Ela deve estar limpa (completa de zeros).
 */
void
gm_matrix_mult( const gm_matrix_t * matrix_A,
		const gm_matrix_t * matrix_X,
		gm_matrix_t * matrix_B )
{
    int i = 0;
    int j = 0;
    int k = 0;

    for( i = 0 ; i < matrix_A->n_row ; i++ )
	for( j = 0 ; j < matrix_X->n_col ; j++ )
	    for( k = 0 ; k < matrix_A->n_col ; k++ )
		matrix_B->matrix[i][j] += matrix_A->matrix[i][k] * matrix_X->matrix[k][j];
}

/** Limpa a matriz inserindo 0s.

    @param[in][out] *matrix Matriz a ser limpa.
*/
void
gm_matrix_clean( gm_matrix_t * matrix )
{
  int i = 0;

  for( i = 0 ; i < matrix->n_row ; i++ )
    memset( matrix->matrix[i], 0, sizeof(float) * matrix->n_col );
}

/** Imprime uma matriz.

    @param *mat ponteiro para uma matriz.
*/
void
gm_matrix_print( gm_matrix_t * mat )
{
  int i = 0;
  int j = 0;

  for( i = 0 ; i < mat->n_row ; i++ )
    {
      for( j = 0 ; j < mat->n_col ; j++ )
	printf("%f  ",mat->matrix[i][j]);
      printf("\n");
    }
}

/** Cria uma nova matriz.
 *
 * @param n_row Número de linhas da nova matriz.
 *
 * @param n_col Número de colunas da nova matriz.
 *
 * @return Novo objeto gm_matrix_t
 */
gm_matrix_t *
gm_matrix_new(const int n_row, const int n_col)
{
    gm_matrix_t * self = NULL;
    int i = 0;

    self = XMALLOC( gm_matrix_t, 1 );

    self->n_row = n_row;
    self->n_col = n_col;

    self->matrix = XMALLOC( float *, n_row );
    for( i = 0 ; i < n_row ; i++ )
    {
	self->matrix[i] = XMALLOC( float, n_col );
	memset( self->matrix[i], 0, sizeof(float) * n_col );
    }
    
    return self;
}

/** Seta o valor em uma posição na matriz.
 *
 * @param *mat Matriz.
 *
 * @param x Linha.
 *
 * @param y Coluna.
 *
 * @param value Novo valor.
 */
void
gm_matrix_set( gm_matrix_t * mat, const int x, const int y, const float value )
{
    mat->matrix[x][y] = value;
}

/** Retorna o valor contido na posição passada.
 *
 * @param *mat Matriz.
 *
 * @param x Linha.
 *
 * @param y Coluna.
 *
 * @return O valor contido na posição requisitada.
 */
float
gm_matrix_get( gm_matrix_t *mat, const int x, const int y )
{
    return mat->matrix[x][y];
}

/** Passa os argumentos passados pela linha de comando. */
int
gm_args_parse( int argc, char *argv[],  gm_args_t * args )
{
    memset( args, 0, sizeof( gm_args_t ) );

    /* Setando a estrutura args com estados padrões. */
    args->server_port = GM_SERVER_DEFAULT_PORT;
    args->server_real = GM_SERVER_DEFAULT_REAL;
    args->f_server_display = GM_SERVER_DEFAULT_F_DISPLAY;
    args->serial_path = GM_SERIAL_DEFAULT_PATH;

    int c;

    printf("GEAR - Grupo de Estudos Avançados em Robótica\n"
	   "           GEARMedula - Servidor\n\n");
    while ((c = getopt (argc, argv, "ps:hRdv")) != -1)
	switch (c)
	{
	case 'v':
	{
	    printf("Versão %s.\n", PACKAGE_VERSION);
	    return 1;
	    break;
	}
	case 's':
	{
	    args->serial_path = gm_utils_strcpy( optarg );		
	    break;
	}
	case 'd':
	    args->f_server_display = 1;
	    break;
	case 'h':
	    printf("Utilize as seguintes opções:\n\n"
		   "-p Porta em que o sevidor ficará escutando (Padrão em "GM_SERVER_DEFAULT_PORT_STR")\n"
		   "-R Servidor em modo Mundo Real. (Simulação é o modo padrão)\n"
		   "-s Caminho para a serial. (Padrão em "GM_SERIAL_DEFAULT_PATH")\n"
		   "-d Ativa o display do servidor\n"
		   "-v Retorna a versão do GEARMedula\n"
		   "-h Esta ajuda.\n");
	    return 1;
	    break;
	case 'R':
	    args->server_real = 1;
	    break;
	case 'p':
	    gm_utils_str2int( &(args->server_port), optarg );
	    break;
	case '?':
	    if (optopt == 'c')
		fprintf (stderr, "Option -%c requires an argument.\n", optopt);
	    else if (isprint (optopt))
		fprintf (stderr, "Unknown option `-%c'.\n", optopt);
	    else
		fprintf (stderr,
			 "Unknown option character `\\x%x'.\n",
			 optopt);
	default:
	    abort ();
	}
    return 0;
}

/** Converte uma string para um inteiro.
 *
 * @param[out] *num Ponteiro para o inteiro.
 *
 * @param[in] *str String a ser convertida.
 */
void
gm_utils_str2int( int * num, char * str )
{
    sscanf( str, "%d", num );
}


/** Converte um inteiro para uma string
 *
 * Observar que é necessário que *str tenha espaço suficiente para
 * alocar o inteiro.
 *
 * @param[out] *str Ponteiro para a string.
 *
 * @param[in] num Inteiro a ser convertido.
 *
 * @param[in] size Tamanho da string que irá receber o inteiro.
 */
void
gm_utils_int2str( char * str, const int num, const int size )
{
    snprintf( str, size, "%d", num );
}

/** Converte um float para uma string
 *
 * Observar que é necessário que *str tenha espaço suficiente para
 * alocar o float.
 *
 * @param[out] *str Ponteiro para a string.
 *
 * @param[in] num Float a ser convertido.
 *
 * @param[in] size Tamanho da string que irá receber o float.
 */
void
gm_utils_float2str( char * str, const float num, const int size )
{
    snprintf( str, size, "%f", num );
}

int
gm_utils_vec_get_next_int( const char * msg, int * iter )
{
    const char *ptr = &(msg[*iter]);
    char field [ 32 ];
    int n;
    int num = -1;
    
    sscanf(ptr, "%31[^|]%n", field, &n);
    sscanf( field, "%d", &num );
    *iter += n+1; /* advance the pointer by the number of characters read */
    return num;
}

float
gm_utils_vec_get_next_float( const char * msg, int * iter )
{
    const char *ptr = &(msg[*iter]);
    char field [ 32 ];
    int n;
    float num = -1;
    
    sscanf(ptr, "%31[^|]%n", field, &n);
    sscanf( field, "%f", &num );
    *iter += n+1; /* advance the pointer by the number of characters read */
    return num;
}

/** Converte metros por segundo para pwm.
 *
 * Utiliza uma regra de 3 tendo um limite chutado.
 */
int
gm_utils_ms2pwm( const float ms )
{

    return 0;
}


/** Copia e retorna uma nova string.
 *
 * @param * buff String a ser copiada
 *
 * @return Uma string copiada e recem criada.
 */
char *
gm_utils_strcpy( const char * buff )
{
    int nbytes = 0;
    char * res = 0;

    nbytes = strlen( buff );
    res = XMALLOC( char, nbytes + 1 );
    strcpy( res, buff );

    return res;
}
